//
// Created by tao on 7/24/21.
//

#include "imu_converter.h"

namespace kaist_publisher {

ImuConverter::ImuConverter(ros::NodeHandle& nh, const std::string& dataset_dir,
                           const std::string& save_dir,
                           const std::string& irp_topic,
                           const std::string& raw_topic,
                           const std::string& mag_topic)
    : Converter(dataset_dir, save_dir),
      irp_topic_(irp_topic),
      raw_topic_(raw_topic),
      mag_topic_(mag_topic) {
    irp_bag_name_ = FilterSlash(irp_topic_) + ".bag";
    raw_bag_name_ = FilterSlash(raw_topic_) + ".bag";
    mag_bag_name_ = FilterSlash(mag_topic_) + ".bag";

    irp_publisher = nh.advertise<irp_sen_msgs::imu>(irp_topic_.c_str(), 40);
    raw_publisher = nh.advertise<sensor_msgs::Imu>(raw_topic_.c_str(), 40);
    mag_publisher =
        nh.advertise<sensor_msgs::MagneticField>(mag_topic_.c_str(), 40);
}

int64_t ImuConverter::Init_Publish() {
    const std::string data_file = dataset_dir_ + "/" + default_data_file;
    fp_ = fopen(data_file.c_str(), "r");

    if (fscanf(fp_,
               "%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%"
               "lf,%lf\n",
               &stamp_, &q_x_, &q_y_, &q_z_, &q_w_, &x_, &y_, &z_, &g_x_, &g_y_,
               &g_z_, &a_x_, &a_y_, &a_z_, &m_x_, &m_y_, &m_z_) == 17) {
        return stamp_;
    } else {
        fclose(fp_);
        return LONG_MAX;
    }
}

int64_t ImuConverter::Publish() {
    irp_sen_msgs::imu irp_imu;
    sensor_msgs::Imu sensor_msgs_imu;
    sensor_msgs::MagneticField sensor_msgs_mag;
    irp_imu.header.stamp.fromNSec(stamp_);
    irp_imu.header.frame_id = "imu";
    irp_imu.quaternion_data.x = q_x_;
    irp_imu.quaternion_data.y = q_y_;
    irp_imu.quaternion_data.z = q_z_;
    irp_imu.quaternion_data.w = q_w_;
    irp_imu.eular_data.x = x_;
    irp_imu.eular_data.y = y_;
    irp_imu.eular_data.z = z_;
    irp_imu.gyro_data.x = g_x_;
    irp_imu.gyro_data.y = g_y_;
    irp_imu.gyro_data.z = g_z_;
    irp_imu.acceleration_data.x = a_x_;
    irp_imu.acceleration_data.y = a_y_;
    irp_imu.acceleration_data.z = a_z_;
    irp_imu.magneticfield_data.x = m_x_;
    irp_imu.magneticfield_data.y = m_y_;
    irp_imu.magneticfield_data.z = m_z_;

    sensor_msgs_imu.header.stamp.fromNSec(stamp_);
    sensor_msgs_imu.header.frame_id = "imu";
    sensor_msgs_imu.orientation.x = q_x_;
    sensor_msgs_imu.orientation.y = q_y_;
    sensor_msgs_imu.orientation.z = q_z_;
    sensor_msgs_imu.orientation.w = q_w_;
    sensor_msgs_imu.angular_velocity.x = g_x_;
    sensor_msgs_imu.angular_velocity.y = g_y_;
    sensor_msgs_imu.angular_velocity.z = g_z_;
    sensor_msgs_imu.linear_acceleration.x = a_x_;
    sensor_msgs_imu.linear_acceleration.y = a_y_;
    sensor_msgs_imu.linear_acceleration.z = a_z_;
    sensor_msgs_imu.orientation_covariance[0] = 3;
    sensor_msgs_imu.orientation_covariance[4] = 3;
    sensor_msgs_imu.orientation_covariance[8] = 3;
    sensor_msgs_imu.angular_velocity_covariance[0] = 3;
    sensor_msgs_imu.angular_velocity_covariance[4] = 3;
    sensor_msgs_imu.angular_velocity_covariance[8] = 3;
    sensor_msgs_imu.linear_acceleration_covariance[0] = 3;
    sensor_msgs_imu.linear_acceleration_covariance[4] = 3;
    sensor_msgs_imu.linear_acceleration_covariance[8] = 3;

    sensor_msgs_mag.header.stamp.fromNSec(stamp_);
    sensor_msgs_mag.header.frame_id = "imu";
    sensor_msgs_mag.magnetic_field.x = m_x_;
    sensor_msgs_mag.magnetic_field.y = m_y_;
    sensor_msgs_mag.magnetic_field.z = m_z_;

    irp_publisher.publish(irp_imu);
    raw_publisher.publish(sensor_msgs_imu);
    mag_publisher.publish(sensor_msgs_mag);

    if (fscanf(fp_,
               "%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%"
               "lf,%lf\n",
               &stamp_, &q_x_, &q_y_, &q_z_, &q_w_, &x_, &y_, &z_, &g_x_, &g_y_,
               &g_z_, &a_x_, &a_y_, &a_z_, &m_x_, &m_y_, &m_z_) == 17) {
        return stamp_;
    }

    fclose(fp_);
    return LONG_MAX;
}

void ImuConverter::Publish(int64_t& timestamp,
                           sensor_msgs::Imu::Ptr& sensor_msgs_imu,
                           sensor_msgs::MagneticField::Ptr& sensor_msgs_mag) {
    irp_sen_msgs::imuPtr irp_imu(new irp_sen_msgs::imu());
    sensor_msgs_imu.reset(new sensor_msgs::Imu());
    sensor_msgs_mag.reset(new sensor_msgs::MagneticField());
    irp_imu->header.stamp.fromNSec(stamp_);
    irp_imu->header.frame_id = "imu";
    irp_imu->quaternion_data.x = q_x_;
    irp_imu->quaternion_data.y = q_y_;
    irp_imu->quaternion_data.z = q_z_;
    irp_imu->quaternion_data.w = q_w_;
    irp_imu->eular_data.x = x_;
    irp_imu->eular_data.y = y_;
    irp_imu->eular_data.z = z_;
    irp_imu->gyro_data.x = g_x_;
    irp_imu->gyro_data.y = g_y_;
    irp_imu->gyro_data.z = g_z_;
    irp_imu->acceleration_data.x = a_x_;
    irp_imu->acceleration_data.y = a_y_;
    irp_imu->acceleration_data.z = a_z_;
    irp_imu->magneticfield_data.x = m_x_;
    irp_imu->magneticfield_data.y = m_y_;
    irp_imu->magneticfield_data.z = m_z_;

    sensor_msgs_imu->header.stamp.fromNSec(stamp_);
    sensor_msgs_imu->header.frame_id = "imu";
    sensor_msgs_imu->orientation.x = q_x_;
    sensor_msgs_imu->orientation.y = q_y_;
    sensor_msgs_imu->orientation.z = q_z_;
    sensor_msgs_imu->orientation.w = q_w_;
    sensor_msgs_imu->angular_velocity.x = g_x_;
    sensor_msgs_imu->angular_velocity.y = g_y_;
    sensor_msgs_imu->angular_velocity.z = g_z_;
    sensor_msgs_imu->linear_acceleration.x = a_x_;
    sensor_msgs_imu->linear_acceleration.y = a_y_;
    sensor_msgs_imu->linear_acceleration.z = a_z_;
    sensor_msgs_imu->orientation_covariance[0] = 3;
    sensor_msgs_imu->orientation_covariance[4] = 3;
    sensor_msgs_imu->orientation_covariance[8] = 3;
    sensor_msgs_imu->angular_velocity_covariance[0] = 3;
    sensor_msgs_imu->angular_velocity_covariance[4] = 3;
    sensor_msgs_imu->angular_velocity_covariance[8] = 3;
    sensor_msgs_imu->linear_acceleration_covariance[0] = 3;
    sensor_msgs_imu->linear_acceleration_covariance[4] = 3;
    sensor_msgs_imu->linear_acceleration_covariance[8] = 3;

    sensor_msgs_mag->header.stamp.fromNSec(stamp_);
    sensor_msgs_mag->header.frame_id = "imu";
    sensor_msgs_mag->magnetic_field.x = m_x_;
    sensor_msgs_mag->magnetic_field.y = m_y_;
    sensor_msgs_mag->magnetic_field.z = m_z_;

    // irp_publisher.publish(irp_imu);
    // raw_publisher.publish(sensor_msgs_imu);
    // mag_publisher.publish(sensor_msgs_mag);

    if (fscanf(fp_,
               "%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%"
               "lf,%lf\n",
               &stamp_, &q_x_, &q_y_, &q_z_, &q_w_, &x_, &y_, &z_, &g_x_, &g_y_,
               &g_z_, &a_x_, &a_y_, &a_z_, &m_x_, &m_y_, &m_z_) == 17) {
        timestamp = stamp_;
        return;
    }

    fclose(fp_);
    timestamp = LONG_MAX;
    return;
}

int ImuConverter::Convert() {
    CheckAndCreateSaveDir();

    boost::filesystem::path irp_bag_file =
        boost::filesystem::path(save_dir_) / irp_bag_name_;
    boost::filesystem::path raw_bag_file =
        boost::filesystem::path(save_dir_) / raw_bag_name_;
    boost::filesystem::path mag_bag_file =
        boost::filesystem::path(save_dir_) / mag_bag_name_;
    // rosbag::Bag irp_bag(irp_bag_file.string(), rosbag::bagmode::Write);
    // rosbag::Bag raw_bag(raw_bag_file.string(), rosbag::bagmode::Write);
    // rosbag::Bag mag_bag(mag_bag_file.string(), rosbag::bagmode::Write);
    // ROS_INFO("saving %s", irp_bag_file.c_str());
    // ROS_INFO("saving %s", raw_bag_file.c_str());
    // ROS_INFO("saving %s", mag_bag_file.c_str());

    const std::string data_file = dataset_dir_ + "/" + default_data_file;
    FILE* fp = fopen(data_file.c_str(), "r");
    irp_sen_msgs::imu irp_imu;
    sensor_msgs::Imu sensor_msgs_imu;
    sensor_msgs::MagneticField sensor_msgs_mag;

    int64_t stamp;
    double q_x, q_y, q_z, q_w, x, y, z, g_x, g_y, g_z, a_x, a_y, a_z, m_x, m_y,
        m_z;
    while (fscanf(fp,
                  "%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,"
                  "%lf,%lf\n",
                  &stamp, &q_x, &q_y, &q_z, &q_w, &x, &y, &z, &g_x, &g_y, &g_z,
                  &a_x, &a_y, &a_z, &m_x, &m_y, &m_z) == 17) {
        irp_imu.header.stamp.fromNSec(stamp);
        irp_imu.header.frame_id = "imu";
        irp_imu.quaternion_data.x = q_x;
        irp_imu.quaternion_data.y = q_y;
        irp_imu.quaternion_data.z = q_z;
        irp_imu.quaternion_data.w = q_w;
        irp_imu.eular_data.x = x;
        irp_imu.eular_data.y = y;
        irp_imu.eular_data.z = z;
        irp_imu.gyro_data.x = g_x;
        irp_imu.gyro_data.y = g_y;
        irp_imu.gyro_data.z = g_z;
        irp_imu.acceleration_data.x = a_x;
        irp_imu.acceleration_data.y = a_y;
        irp_imu.acceleration_data.z = a_z;
        irp_imu.magneticfield_data.x = m_x;
        irp_imu.magneticfield_data.y = m_y;
        irp_imu.magneticfield_data.z = m_z;

        sensor_msgs_imu.header.stamp.fromNSec(stamp);
        sensor_msgs_imu.header.frame_id = "imu";
        sensor_msgs_imu.orientation.x = q_x;
        sensor_msgs_imu.orientation.y = q_y;
        sensor_msgs_imu.orientation.z = q_z;
        sensor_msgs_imu.orientation.w = q_w;
        sensor_msgs_imu.angular_velocity.x = g_x;
        sensor_msgs_imu.angular_velocity.y = g_y;
        sensor_msgs_imu.angular_velocity.z = g_z;
        sensor_msgs_imu.linear_acceleration.x = a_x;
        sensor_msgs_imu.linear_acceleration.y = a_y;
        sensor_msgs_imu.linear_acceleration.z = a_z;
        sensor_msgs_imu.orientation_covariance[0] = 3;
        sensor_msgs_imu.orientation_covariance[4] = 3;
        sensor_msgs_imu.orientation_covariance[8] = 3;
        sensor_msgs_imu.angular_velocity_covariance[0] = 3;
        sensor_msgs_imu.angular_velocity_covariance[4] = 3;
        sensor_msgs_imu.angular_velocity_covariance[8] = 3;
        sensor_msgs_imu.linear_acceleration_covariance[0] = 3;
        sensor_msgs_imu.linear_acceleration_covariance[4] = 3;
        sensor_msgs_imu.linear_acceleration_covariance[8] = 3;

        sensor_msgs_mag.header.stamp.fromNSec(stamp);
        sensor_msgs_mag.header.frame_id = "imu";
        sensor_msgs_mag.magnetic_field.x = m_x;
        sensor_msgs_mag.magnetic_field.y = m_y;
        sensor_msgs_mag.magnetic_field.z = m_z;

        // irp_bag.write(irp_topic_, irp_imu.header.stamp, irp_imu);
        // raw_bag.write(raw_topic_, sensor_msgs_imu.header.stamp,
        // sensor_msgs_imu); mag_bag.write(mag_topic_,
        // sensor_msgs_mag.header.stamp, sensor_msgs_mag);
        irp_publisher.publish(irp_imu);
        raw_publisher.publish(sensor_msgs_imu);
        mag_publisher.publish(sensor_msgs_mag);
    }
    // irp_bag.close();
    // raw_bag.close();
    // mag_bag.close();
    fclose(fp);
    // ROS_INFO("done saving %s", irp_bag_file.c_str());
    // ROS_INFO("done saving %s", raw_bag_file.c_str());
    // ROS_INFO("done saving %s", mag_bag_file.c_str());

    return 0;
}

}  // namespace kaist_publisher